"""
舵机协议模块

该模块实现了舵机通信的协议。
"""

import time
import logging
from typing import Optional, Tuple, Union, List
from .serial_protocol import SerialProtocol


class ServoCommand:
    """舵机命令定义"""

    # 基础命令
    FRAME_HEADER = 0x55  # 帧头
    MOVE_TIME_WRITE = 1  # 移动时间写入
    MOVE_TIME_READ = 2  # 移动时间读取
    MOVE_TIME_WAIT_WRITE = 7  # 移动时间等待写入
    MOVE_TIME_WAIT_READ = 8  # 移动时间等待读取
    MOVE_START = 11  # 移动开始
    MOVE_STOP = 12  # 移动停止
    ID_WRITE = 13  # ID写入
    ID_READ = 14  # ID读取
    ANGLE_OFFSET_ADJUST = 17  # 角度偏移调整
    ANGLE_OFFSET_WRITE = 18  # 角度偏移写入
    ANGLE_OFFSET_READ = 19  # 角度偏移读取
    ANGLE_LIMIT_WRITE = 20  # 角度限制写入
    ANGLE_LIMIT_READ = 21  # 角度限制读取
    VIN_LIMIT_WRITE = 22  # 电压限制写入
    VIN_LIMIT_READ = 23  # 电压限制读取
    TEMP_MAX_LIMIT_WRITE = 24  # 温度最大限制写入
    TEMP_MAX_LIMIT_READ = 25  # 温度最大限制读取
    TEMP_READ = 26  # 温度读取
    VIN_READ = 27  # 电压读取
    POS_READ = 28  # 位置读取
    OR_MOTOR_MODE_WRITE = 29  # 正反转电机模式写入
    OR_MOTOR_MODE_READ = 30  # 正反转电机模式读取
    LOAD_OR_UNLOAD_WRITE = 31  # 负载或卸载写入
    LOAD_OR_UNLOAD_READ = 32  # 负载或卸载读取
    LED_CTRL_WRITE = 33  # LED控制写入
    LED_CTRL_READ = 34  # LED控制读取
    LED_ERROR_WRITE = 35  # LED错误写入
    LED_ERROR_READ = 36  # LED错误读取


class ServoProtocol:
    """舵机协议类"""

    def __init__(self, protocol: SerialProtocol, log_level: int = logging.INFO):
        """
        初始化舵机协议

        Args:
            protocol: 底层通信协议
            log_level: 日志级别
        """
        self.logger = logging.getLogger(__name__)
        self.logger.setLevel(log_level)  # 设置当前模块的日志级别
        self.protocol = protocol
        self.is_connected = False

    def connect(self) -> bool:
        """
        建立连接

        Returns:
            bool: 连接是否成功
        """
        if not self.protocol:
            self.logger.error("底层通信协议未初始化")
            return False

        try:
            self.is_connected = self.protocol.connect()
            return self.is_connected
        except Exception as e:
            self.logger.error(f"连接失败: {e}")
            return False

    def disconnect(self) -> None:
        """断开连接"""
        self.protocol.disconnect()
        self.is_connected = False

    def write_command(
        self, servo_id: int, cmd: int, data: List[int], retry_count: int = 3
    ) -> bool:
        """
        写入命令

        Args:
            servo_id: 舵机ID
            cmd: 命令类型
            data: 命令数据
            retry_count: 重试次数

        Returns:
            bool: 是否写入成功
        """
        return self.protocol.write_command(servo_id, cmd, data, retry_count)

    def read_command(self, servo_id: int, cmd: int, retry_count: int = 3) -> bool:
        """
        发送读取命令

        Args:
            servo_id: 舵机ID
            cmd: 命令类型
            retry_count: 重试次数

        Returns:
            bool: 命令是否发送成功
        """
        return self.protocol.read_command(servo_id, cmd, retry_count)

    def get_response(
        self, cmd: int, timeout: float = 0.1
    ) -> Optional[Union[int, Tuple[int, int]]]:
        """
        获取响应数据

        Args:
            cmd: 命令类型
            timeout: 超时时间(秒)

        Returns:
            响应数据，如果读取失败则返回None
        """
        return self.protocol.get_response(cmd, timeout)

    def set_servo_id(self, old_id: int, new_id: int) -> bool:
        """
        设置舵机ID

        Args:
            old_id: 当前ID
            new_id: 新ID

        Returns:
            bool: 是否设置成功
        """
        try:
            self.logger.debug(f"开始设置舵机ID: {old_id} -> {new_id}")

            # 1. 配置串口为输出模式
            self.logger.debug("已配置串口为输出模式")

            # 2. 发送ID写入命令
            if not self.write_command(old_id, ServoCommand.ID_WRITE, [new_id]):
                self.logger.error("发送ID写入命令失败")
                return False

            self.logger.debug("已发送ID写入命令")

            self.protocol.port_reset()

            # 3. 等待舵机响应
            time.sleep(0.2)  # 增加等待时间到200ms

            # 4. 验证新ID
            self.logger.debug("开始验证新ID")
            if not self.read_command(new_id, ServoCommand.ID_READ):
                self.logger.error("发送ID读取命令失败")
                return False

            # 5. 获取响应
            response = self.get_response(ServoCommand.ID_READ)
            if response is None:
                self.logger.error("获取ID响应失败")
                return False

            # 6. 验证响应
            if response == new_id:
                self.logger.info(f"舵机ID设置成功: {old_id} -> {new_id}")
                return True
            else:
                self.logger.error(f"ID验证失败: 期望 {new_id}, 实际 {response}")
                return False

        except Exception as e:
            import traceback

            traceback.print_exc()
            self.logger.error(f"设置舵机ID时发生错误: {str(e)}")
            return False

    def get_servo_id(self) -> Optional[int]:
        """
        获取舵机ID

        Returns:
            Optional[int]: 舵机ID，如果获取失败则返回None
        """
        if self.read_command(0xFE, ServoCommand.ID_READ):
            return self.get_response(ServoCommand.ID_READ)
        return None

    def set_servo_angle(self, servo_id: int, angle: int, time_ms: int = 1000) -> bool:
        """
        设置舵机角度

        Args:
            servo_id: 舵机ID
            angle: 目标角度
            time_ms: 运动时间(毫秒)

        Returns:
            bool: 是否设置成功
        """
        return self.write_command(
            servo_id, ServoCommand.MOVE_TIME_WRITE, [angle, time_ms]
        )

    def get_servo_angle(self, servo_id: int) -> Optional[int]:
        """
        获取舵机当前角度

        Args:
            servo_id: 舵机ID

        Returns:
            Optional[int]: 当前角度，如果获取失败则返回None
        """
        if self.read_command(servo_id, ServoCommand.POS_READ):
            return self.get_response(ServoCommand.POS_READ)
        return None

    def set_servo_offset(self, servo_id: int, offset: int) -> bool:
        """
        设置舵机偏移量

        Args:
            servo_id: 舵机ID
            offset: 偏移量

        Returns:
            bool: 是否设置成功
        """
        if not self.write_command(servo_id, ServoCommand.ANGLE_OFFSET_ADJUST, [offset]):
            return False

        time.sleep(0.1)
        return self.write_command(servo_id, ServoCommand.ANGLE_OFFSET_WRITE, [])

    def get_servo_offset(self, servo_id: int) -> Optional[int]:
        """
        获取舵机偏移量

        Args:
            servo_id: 舵机ID

        Returns:
            Optional[int]: 偏移量，如果获取失败则返回None
        """
        if self.read_command(servo_id, ServoCommand.ANGLE_OFFSET_READ):
            return self.get_response(ServoCommand.ANGLE_OFFSET_READ)
        return None

    def set_servo_angle_limit(
        self, servo_id: int, min_angle: int, max_angle: int
    ) -> bool:
        """
        设置舵机角度限制

        Args:
            servo_id: 舵机ID
            min_angle: 最小角度
            max_angle: 最大角度

        Returns:
            bool: 是否设置成功
        """
        return self.write_command(
            servo_id, ServoCommand.ANGLE_LIMIT_WRITE, [min_angle, max_angle]
        )

    def get_servo_angle_limit(self, servo_id: int) -> Optional[Tuple[int, int]]:
        """
        获取舵机角度限制

        Args:
            servo_id: 舵机ID

        Returns:
            Optional[Tuple[int, int]]: (最小角度, 最大角度)，如果获取失败则返回None
        """
        if self.read_command(servo_id, ServoCommand.ANGLE_LIMIT_READ):
            response = self.get_response(ServoCommand.ANGLE_LIMIT_READ)
            if isinstance(response, tuple) and len(response) == 2:
                return response
        return None

    def get_servo_voltage(self, servo_id: int) -> Optional[int]:
        """
        获取舵机电压

        Args:
            servo_id: 舵机ID

        Returns:
            Optional[int]: 电压值(mV)，如果获取失败则返回None
        """
        if self.read_command(servo_id, ServoCommand.VIN_READ):
            return self.get_response(ServoCommand.VIN_READ)
        return None

    def get_servo_temperature(self, servo_id: int) -> Optional[int]:
        """
        获取舵机温度

        Args:
            servo_id: 舵机ID

        Returns:
            Optional[int]: 温度值(℃)，如果获取失败则返回None
        """
        if self.read_command(servo_id, ServoCommand.TEMP_READ):
            return self.get_response(ServoCommand.TEMP_READ)
        return None
